from serial.tools import list_ports

from ..pydobotkeo.dobot import Dobot
import time

port = list_ports.comports()[0].device
device = Dobot(port=port, verbose=False)
device._set_queued_cmd_clear()
device.go_home()
# device.move_to(-191 ,0, 172, 0, wait=True)
# while(1):
# (x, y, z, r, j1, j2, j3, j4) = device.pose()
# device
# device.move_to(191 ,0, 172, 0, wait=False)
# device.suck(0)
# print(device.pose()[:])
# print(f'x:{x} y:{y} z:{z}')
base_x = 200
grab_position_adjust = 27
grab_x = 100 + grab_position_adjust + base_x
grab_y = 100
grab_z = 150#-31
(x, y, z, r, j1, j2, j3, j4) = device.pose()
print(f'x:{x} y:{y} z:{z} j1:{j1} j2:{j2} j3:{j3} j4:{j4}')
device.go_grab_home()  # 200, 0
# device.move_to(300 , -83, grab_z, r, wait=True) #first box relative -101,247
device.move_to(200, grab_y, grab_z, r, wait=True)
# device.suck(True)
# time.sleep(1)
device.go_grab_home()
# device.move_to(180 , 180, 150, r, wait=True)
# device.move_to(80 , 200, 150, r, wait=True)
# device.move_to(180 , 180, 150, r, wait=True)
# time.sleep(3)
# device.go_grab_home()
# device.move_to(grab_x, grab_y, grab_z, r, wait=True)
# device.move_to(300 , -83, , r, wait=True)
# device.suck(False)
# device.go_grab_home()
# device.move_to(x, y, z, r, wait=True)  # we wait until this movement is done before continuing

device.close()
